package simulator.labyrinth;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Move.MoveType;

import Program.Condition;
import Program.MathExtra;

import simulator.labyrinth.Square.SquareState;
import simulator.position.Point;
import simulator.position.Position;


public class Labyrinth {

	private static final int SECTOR_SIZE = 40;
	// The key value is the upper left corner of the square
	private HashMap<Point, Square> squares = new HashMap<Point, Square>();
	private PathSection currentSection;

	public void addSquare(Point position, Square square) {
		squares.put(position, square);
	}
	
	/**
	 * Adds a wall to the corresponding squares. Does nothing if wall is null.
	 * 
	 * @param wall
	 */
	public void addWall(Wall wall) {
		if (wall == null)
			return;
		
		Point positionOfFirstSquare = wall.getStartPosition();
		Square square1 = getSquareFromPoint(positionOfFirstSquare);
		
		if (square1 != null && square1.getWalls().contains(wall))
			return;
		
		Point positionOfSecondSquare;
		
		double orientation = wall.getOrientation();
		
		if (Double.compare(orientation,0) == 0)
			positionOfSecondSquare = getStartPointOfSquare(new Point(positionOfFirstSquare.getX(), positionOfFirstSquare.getY() - 1));
		else
			positionOfSecondSquare = getStartPointOfSquare(new Point(positionOfFirstSquare.getX() - 1, positionOfFirstSquare.getY()));

		Square square2 = getSquareFromPoint(positionOfSecondSquare);
		
		if (square1 == null) {
			square1 = new Square();
			squares.put(positionOfFirstSquare, square1);
		}
		if (square2 == null) {
			square2 = new Square();
			squares.put(positionOfSecondSquare, square2);
		}
		
		square1.addWall(wall);
		square2.addWall(wall);
	}
	
	public static Point getStartPointOfSquare(Point point) {
		int xDiff = ((point.getX() % SECTOR_SIZE) + SECTOR_SIZE) % SECTOR_SIZE;
		int yDiff = ((point.getY() % SECTOR_SIZE) + SECTOR_SIZE) % SECTOR_SIZE;
		return new Point(point.getX() - xDiff, point.getY() - yDiff);
	}
	
	/**
	 * 
	 * @param square
	 * @return The startPoint of the square or null if this Labyrinth
	 * 			does not contain the square.
	 */
	public Point getStartPointOfSquare(Square square) {
		for (Point point: squares.keySet()) {
			if (squares.get(point).equals(square))
				return point;
		}
		
		return null;
	}
	
	public Square getSquareFromPoint(Point point) {
		Point p = getStartPointOfSquare(point);
		return squares.get(p);
	}
	
	public Square getAdjacentSquareGoing(Position current, double rotation) {
		Position pos = current.clone();
		
		pos.turn(MathExtra.getDiscreteAngle(rotation));
		pos.move(SECTOR_SIZE);

		return getSquareFromPoint(pos.getPoint());
	}
	
	public List<Square> getSquares() {
		return new ArrayList<Square>(squares.values());
	}

	public void addBarcode(Point topLeft, BarCode barCode) {
		Square square = new Square();
		if (this.getSquareFromPoint(topLeft) != null)
			square = this.getSquareFromPoint(topLeft);
		square.addBarCode(barCode);
		squares.put(getStartPointOfSquare(topLeft), square);
	}

	public void addLine(Line line) {
		Point positionOfFirstSquare = line.getStartPosition();
		Square square1 = getSquareFromPoint(positionOfFirstSquare);

		if (square1 != null && square1.getLines().contains(line))
			return;

		Point positionOfSecondSquare;

		double orientation = line.getOrientation();

		if (Double.compare(orientation,0) == 0)
			positionOfSecondSquare = getStartPointOfSquare(new Point(positionOfFirstSquare.getX(), positionOfFirstSquare.getY() - 1));
		else
			positionOfSecondSquare = getStartPointOfSquare(new Point(positionOfFirstSquare.getX() - 1, positionOfFirstSquare.getY()));

		Square square2 = getSquareFromPoint(positionOfSecondSquare);

		if (square1 == null) {
			square1 = new Square();
			squares.put(positionOfFirstSquare, square1);
		}
		if (square2 == null) {
			square2 = new Square();
			squares.put(positionOfSecondSquare, square2);
		}

		square1.addLine(line);
		square2.addLine(line);
	}
	
	/**
	 * 
	 * @param	condition
	 * 			De conditie waaraan de vakjes moeten voldoen.
	 * @return	Een lijst met alle vakjes die aan de gegeven conditie voldoen
	 * 			en die in dit labyrinth zitten.
	 */
	public List<Square> getAllSquaresSatisfying(Condition<Square> condition) {
		List<Square> result = new ArrayList<Square>();
		
		for (Square square: squares.values()) {
			if (condition.satisfiesCondition(square))
				result.add(square);
		}
		
		return result;
	}

	/**
	 * Een wrapper-klasse om de informatie, gebruikt voor het zoekalgoritme, 
	 * voor een square op te slaan.
	 * Deze klasse houdt de orientatie bij die de robot heeft op deze square.
	 * Daarnaast ook de acties die de robot moet doen om op deze square te komen.
	 *
	 */
	private class PathSection {
		
		private final int orientation;
		private final List<Move> actionList;
		
		public PathSection(int orientation, List<Move> actionList) {
			this.orientation = orientation;
			this.actionList = actionList;
		}

		/**
		 * @return the orientation
		 */
		public int getOrientation() {
			return orientation;
		}

		/**
		 * @return the actionList
		 */
		public List<Move> getActionList() {
			return actionList;
		}
		
		
	}
	
	/**
	 * 
	 * @param 	pos
	 * 			de huidige positie van de robot
	 * @param	cond
	 * 			de conditie waar het vakje aan moet voldoen.
	 * @param	discover
	 * 			True als het gevonden vakje ontdekt moet worden.
	 * 
	 * @return	A list of moves to go to the closest square to the current position
	 * 			and satisfying the given condition. All moves are not moving except
	 * 			the last move. (for travelsafely) if discover is true. Else none is
	 * 			moving.
	 * 			This method checks for walls.
	 * 			Null if there is no such square found.
	 */
	public List<Move> getPathToClosestSquareSatisfying(Position pos, Condition<Square> cond, boolean discover) {

		List<Square> toCheck  = new ArrayList<Square>();
		// een map die per square een pathsection bijhoudt
		Map<Square, PathSection> actionMap = new HashMap<Square, PathSection>();
		Square current = getSquareFromPoint(pos.getPoint());
		toCheck.add(current);
		actionMap.put(current, new PathSection((int)Math.round(pos.getOrientation()), new ArrayList<Move>()));
		int index = 0;
		
		while(index < toCheck.size()) {
			
			// haal het huidige vakje en bijhorende informatie op.
			current							= toCheck.get(index);
			currentSection		= actionMap.get(current);
			int currentOr					= currentSection.getOrientation();
			List<Move> currentActionList	= currentSection.getActionList();
			Point start						= getStartPointOfSquare(current);
			
			// bekijk de buren van deze square in de volgorde: vooruit, links, rechts, achteruit
			for (int i = 1; i < 5; i++) {
				
				int or = 90 * (i / 2) * (int)Math.pow(-1, i);
				
				Square candidate = getAdjacentSquareGoing(new Position(start.getX() + SECTOR_SIZE / 2, 
																start.getY() + SECTOR_SIZE / 2, 0), or);
				
				if (isWallBetween(current, candidate))
					continue;

				// update de lijst met acties om naar candidate te rijden
				List<Move> newAction = new ArrayList<Move>(currentActionList);
				
				float toTurn = (float)((or - currentOr)) % 360;
				int discreteToTurn = MathExtra.getDiscreteAngle(toTurn);
				//	-90		->		270
				// -270		->		90	
				
				float toTravel = SECTOR_SIZE;
				switch(discreteToTurn) {
					case 90:
						if (toTurn < 0)	// toTurn is ongeveer -270
							newAction.add(new Move(MoveType.ROTATE, 0f, toTurn + 360, false));
						else			// toTurn is ongeveer 90
							newAction.add(new Move(MoveType.ROTATE, 0f, toTurn, false));
						break;
					case 180:	// rij achteruit ipv te draaien.
						or = currentOr;
						toTravel *= -1;
						break;
					case 270:	// draai 90 ipv 270.
						if (toTurn > 0) // toTurn is ongeveer 270
							newAction.add(new Move(MoveType.ROTATE, 0f, toTurn - 360, false));
						else			// toTurn is ongeveer -90
							newAction.add(new Move(MoveType.ROTATE, 0f, toTurn, false));
						break;
				}
				
				newAction.add(new Move(MoveType.TRAVEL, toTravel, 0f, false));
				
				if (cond.satisfiesCondition(candidate)) {
					
					currentSection = new PathSection(or, newAction);
					if (discover) {
						Move lastMove = newAction.get(newAction.size() - 1);
						lastMove.setValues(lastMove.getMoveType(), lastMove.getDistanceTraveled(),
								lastMove.getAngleTurned(), true);
					}
					
					return newAction;
				} else if (candidate != null && candidate.getState().equals(SquareState.DISCOVERED))
					// als de queue candidate nog niet bevat voeg hem toe en zet ook zijn informatie
					// in de map.
					if (!toCheck.contains(candidate)) {
						toCheck.add(candidate);
						// or is de orientatie van de robot op candidate
						actionMap.put(candidate, new PathSection(or, newAction));
					}
			}
			
			index++;
			
		}
		
		return null;
	}
	
	/**
	 * 
	 * @param	pos
	 * 			De huidige positie van de robot.
	 * 
	 * @return	Het kortste pad door alle checkpoints naar de finish.
	 * 			Een leeg pad als de gegeven positie null is.
	 * 
	 * @throws	IllegalStateException
	 * 			Als er geen of meerdere finishes gevonden zijn.
	 */
	public List<Move> getShortestPathToFinish(Position pos) throws IllegalStateException {
		
		if (pos == null)
			return new ArrayList<Move>();
		
		List<Square> checkPoints = getAllSquaresSatisfying(CheckPointSquare.getInstance());
		List<Square> finishes = getAllSquaresSatisfying(FinishSquare.getInstance());
		
		if (finishes.size() != 1) {
			if (finishes.size() == 0)
				throw new IllegalStateException("No finish found.");
			else
				throw new IllegalStateException("Multiple finishes found.");
		}
		
		Square finish = finishes.get(0);
		
		List<Move> shortestPath = null;
		List<List<Square>> checkPointsPerms = MathExtra.permutations(checkPoints);
		
		// bereken de paden voor alle mogelijke permutaties van de checkpoints
		for (List<Square> possiblePath: checkPointsPerms) {

			// voeg de finish toe aan het einde van het pad
			possiblePath.add(finish);
			
			// bereken het pad van huidige positie tot eerste checkpoint
			Square prevCheckPoint = possiblePath.remove(0);
			List<Move> path = getPathToClosestSquareSatisfying(pos, new SpecificSquare(prevCheckPoint), false);
			
			// voeg paden van checkpoint tot checkpoint toe (laatste checkpoint is de finish)
			for (Square checkPoint: possiblePath) {
				Point pointCheckPoint = getStartPointOfSquare(prevCheckPoint);
				double x = pointCheckPoint.getX();
				double y = pointCheckPoint.getY();
				Position posCheckPoint = new Position(x + SECTOR_SIZE / 2* Math.signum(x), y + SECTOR_SIZE / 2 * Math.signum(y), 
						currentSection.orientation);
				
				path.addAll(getPathToClosestSquareSatisfying(posCheckPoint, new SpecificSquare(checkPoint), false));
				// als het pad groter is dan het kortste pad stop met berekenen.
				if (shortestPath != null)
					if (path.size() >= shortestPath.size())
						break;
				
				prevCheckPoint = checkPoint;
			}
			
			// als het huidige pad kleiner is dan kortste pad of als kortste pad null is
			if (shortestPath == null || path.size() < shortestPath.size())
				shortestPath = path;
		}
		
		return shortestPath;
	}
	
	/**
	 * 
	 * @param check
	 * 			Het ene vakje om te checken
	 * @param other
	 * 			Het andere vakje.
	 * 
	 * @return True als en slechts als er een muur tussen beide vakjes staat.
	 */
	public boolean isWallBetween(Square check, Square other) {
		try {
			if (!check.equals(other))
				for (Wall w1 : check.getWalls())
					if (other.getWalls().contains(w1))
						return true;
		} catch (NullPointerException e) {
			// doe niets
		}
						
		return false;
	}
}



class FinishSquare implements Condition<Square> {

	private static final FinishSquare instance = new FinishSquare();
	
	public static FinishSquare getInstance() {
		return instance;
	}
	
	private FinishSquare() {
			
	}
	
	@Override
	public boolean satisfiesCondition(Square toCheck) {
		return toCheck != null && toCheck.isFinish();
	}
	
};

class CheckPointSquare implements Condition<Square> {

	private static final CheckPointSquare instance = new CheckPointSquare();
	
	public static CheckPointSquare getInstance() {
		return instance;
	}
	
	private CheckPointSquare() {
			
	}
	
	@Override
	public boolean satisfiesCondition(Square toCheck) {
		return toCheck != null && toCheck.isCheckPoint();
	}
	
};

class SpecificSquare implements Condition<Square> {
	
	private final Square destination;
	
	public SpecificSquare(Square square) {
		destination = square;
	}
	
	@Override
	public boolean satisfiesCondition(Square toCheck) {
		return toCheck!= null && toCheck.equals(destination);
	}
};
